/*
 * vorwaertskinematik.c
 *
 *  Created on: 14.06.2009
 *      Author: michi
 */
#include "../vars.h"
#include "../glvars.h"
void ermittelTheta() {
	double old;
	double i;
	int faktor;
        if(theta_1 > 2 *M_PI || theta_1 < - 2*M_PI){
          faktor = theta_1 / (2*M_PI);
          theta_1 = theta_1 - (double)(faktor * 2*M_PI);
        }
	aktualisiereMatrixFuerTheta4();
	theta_4 = newton(ermittelTheta_4, theta_4, theta_4);
	theta_2 = newton(ermittelTheta_2, theta_2, theta_2);
	theta_5 = newton(ermittelTheta_5, theta_5, theta_5);
	old = theta_3;
	i = 0;
	theta_3 = newton(ermittelTheta_3, theta_3, theta_3);
	do {
		theta_3 = newton(ermittelTheta_3, theta_3 - 0.01 * i *richtung, theta_3 - 0.01 * i *richtung);
		i++;
               
	} while (fabsl(theta_3) > fabsl(old) + 1.5);

	old = theta_6;
	i = 0;
	theta_6 = newton(ermittelTheta_6, theta_6, theta_6);
	do {
		theta_6 = newton(ermittelTheta_6, theta_6 - 0.01 * i *richtung, theta_6 - 0.01 * i  *richtung);
		i++;
               
	} while (fabsl(theta_6) > fabsl(old) + 1.5);


//theta_7 = ermittelTheta_7();
theta_7 = theta_6;
printf(
			"R: %i T1inc: %f Theta1: %f, Theta2: %f, Theta3: %f, Theta4: %f, Theta5: %f, Theta6: %f, Theta7: %f\n",richtung, speed,
			theta_1, theta_2, theta_3, theta_4, theta_5, theta_6, theta_7);
}
